/**********************************************************************
 Copyright (c) 2020-2024, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/

#include "unitree_lidar_ros2.h"

///////////////////////////////////////////////////////////////////

UnitreeLidarSDKNode::UnitreeLidarSDKNode(const rclcpp::NodeOptions &options)
    : Node("unitree_lidar_sdk_node", options)
{
    // 加载配置参数
    declare_parameter<int>("initialize_type", 1);
    declare_parameter<int>("work_mode", 8);
    declare_parameter<bool>("use_system_timestamp", true);
    declare_parameter<double>("range_min", 0.0);
    declare_parameter<double>("range_max", 50.0);
    declare_parameter<int>("cloud_scan_num", 18);

    declare_parameter<std::string>("serial_port", "/dev/ttyACM0");
    declare_parameter<int>("baudrate", 4000000);

    declare_parameter<int>("lidar_port", 6101);
    declare_parameter<std::string>("lidar_ip", "192.168.1.2");
    declare_parameter<int>("local_port", 6201);
    declare_parameter<std::string>("local_ip", "192.168.1.62");

    declare_parameter<std::string>("cloud_frame", "unilidar_lidar");
    declare_parameter<std::string>("cloud_topic", "unilidar/cloud");

    declare_parameter<std::string>("imu_frame", "unilidar_imu");
    declare_parameter<std::string>("imu_topic", "unilidar/imu");

    // 添加过滤参数
    declare_parameter<double>("filtered_cloud_z_min", -std::numeric_limits<double>::infinity());
    declare_parameter<double>("filtered_cloud_z_max", 2.0);
    declare_parameter<double>("obs_max_detection_range", 1.5);
    declare_parameter<double>("obs_min_detection_range_xy", 0.05);
    declare_parameter<double>("obs_horizontal_fov_deg", 120.0);

    // 新增可调参数
    declare_parameter<int64_t>("buffer_size", 5);

    // 新增：PCL 算法参数声明，并设置默认值
    declare_parameter<double>("ground_segmentation_distance_threshold", 0.015);
    declare_parameter<int>("statistical_outlier_removal_mean_k", 30);
    declare_parameter<double>("statistical_outlier_removal_stddev_thresh", 1.0);
    declare_parameter<double>("euclidean_cluster_tolerance", 0.02);
    declare_parameter<int>("euclidean_cluster_min_size", 80);
    declare_parameter<int>("euclidean_cluster_max_size", 10000);
    // =================================================================


    work_mode_ = get_parameter("work_mode").as_int();
    initialize_type_ = get_parameter("initialize_type").as_int();

    serial_port_ = get_parameter("serial_port").as_string();
    baudrate_ = get_parameter("baudrate").as_int();

    lidar_port_ = get_parameter("lidar_port").as_int();
    lidar_ip_ = get_parameter("lidar_ip").as_string();
    local_port_ = get_parameter("local_port").as_int();
    local_ip_ = get_parameter("local_ip").as_string();

    cloud_scan_num_ = get_parameter("cloud_scan_num").as_int();
    use_system_timestamp_ = get_parameter("use_system_timestamp").as_bool();
    range_max_ = get_parameter("range_max").as_double();
    range_min_ = get_parameter("range_min").as_double();

    cloud_frame_ = get_parameter("cloud_frame").as_string();
    cloud_topic_ = get_parameter("cloud_topic").as_string();
    
    imu_frame_ = get_parameter("imu_frame").as_string();
    imu_topic_ = get_parameter("imu_topic").as_string();

    // 获取过滤参数
    filtered_cloud_z_min_ = get_parameter("filtered_cloud_z_min").as_double();
    filtered_cloud_z_max_ = get_parameter("filtered_cloud_z_max").as_double();
    obs_max_detection_range_ = get_parameter("obs_max_detection_range").as_double();
    obs_min_detection_range_xy_ = get_parameter("obs_min_detection_range_xy").as_double();
    obs_horizontal_fov_deg_ = get_parameter("obs_horizontal_fov_deg").as_double();

    // 获取新增参数
    kBufferSize = static_cast<size_t>(get_parameter("buffer_size").as_int());
    // RCLCPP_INFO(this->get_logger(), "缓冲区大小设置为: %zu", kBufferSize); // 使用 %zu 格式化 size_t

    // 新增：获取 PCL 算法参数值并存入成员变量
    ground_segmentation_distance_threshold_ = get_parameter("ground_segmentation_distance_threshold").as_double();
    statistical_outlier_removal_mean_k_ = get_parameter("statistical_outlier_removal_mean_k").as_int();
    statistical_outlier_removal_stddev_thresh_ = get_parameter("statistical_outlier_removal_stddev_thresh").as_double();
    euclidean_cluster_tolerance_ = get_parameter("euclidean_cluster_tolerance").as_double();
    euclidean_cluster_min_size_ = get_parameter("euclidean_cluster_min_size").as_int();
    euclidean_cluster_max_size_ = get_parameter("euclidean_cluster_max_size").as_int();
    
    RCLCPP_INFO(this->get_logger(), "PCL params loaded: ground_thresh=%.3f, sor_mean_k=%d, sor_std_thresh=%.1f, cluster_tol=%.2f, cluster_min=%d, cluster_max=%d",
        ground_segmentation_distance_threshold_,
        statistical_outlier_removal_mean_k_,
        statistical_outlier_removal_stddev_thresh_,
        euclidean_cluster_tolerance_,
        euclidean_cluster_min_size_,
        euclidean_cluster_max_size_);
    // =================================================================


    // Initialize UnitreeLidarReader
    lsdk_ = createUnitreeLidarReader();


    // ********************** 新增TF转换逻辑 ********************* //
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    // *********************************************************** //

    std::cout << "initialize_type_ = " << initialize_type_ << std::endl;
    RCLCPP_INFO(this->get_logger(), "Node starting with initialize_type: %d", initialize_type_);

    // 初始化传感器
    if (initialize_type_ == 1)
    {
        std::cout << "Attempting to initialize serial port: " << serial_port_ << " with baudrate: " << baudrate_ << std::endl;
        RCLCPP_INFO(this->get_logger(), "Attempting to initialize serial port: %s with baudrate: %d", 
                    serial_port_.c_str(), baudrate_);
        lsdk_->initializeSerial(serial_port_, baudrate_, cloud_scan_num_, use_system_timestamp_, range_min_, range_max_);
        std::cout << "Serial port initialization called (success not verified)" << std::endl;
        RCLCPP_INFO(this->get_logger(), "Serial port initialization called (success not verified)");
    }
    else if (initialize_type_ == 2)
    {
        lsdk_->initializeUDP(lidar_port_, lidar_ip_, local_port_, local_ip_,
                             cloud_scan_num_, use_system_timestamp_, range_min_, range_max_);
    }
    else
    {
        std::cout << "initialize_type is not right! exit now ...\n";
        RCLCPP_ERROR(this->get_logger(), "Invalid initialize_type: %d, exiting", initialize_type_);
        exit(0);
    }

    lsdk_->setLidarWorkMode(work_mode_);
    lsdk_->startLidarRotation(); // 显式启动雷达旋转

    // ROS2发布器初始化
    broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
    pub_cloud_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(cloud_topic_, 10);
    pub_imu_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_topic_, 10);
    pub_filtered_cloud_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("unilidar/filtered_cloud", 10); // 过滤后的点云订阅
    pub_obstacles_ = this->create_publisher<geometry_msgs::msg::PoseArray>("unilidar/obstacles", 10);           // 障碍物位置发布器
    pub_marker_ = this->create_publisher<visualization_msgs::msg::Marker>("unilidar/obstacle_markers", 10);     // 边界框Marker发布器
    timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&UnitreeLidarSDKNode::timer_callback, this));

}



UnitreeLidarSDKNode::~UnitreeLidarSDKNode()
{
    RCLCPP_INFO(this->get_logger(), "Node shutting down, stopping lidar rotation...");
    if (lsdk_)
    {
        lsdk_->stopLidarRotation(); // 停止雷达旋转
        if (initialize_type_ == 1)
        {
            lsdk_->closeSerial();
            RCLCPP_INFO(this->get_logger(), "Serial connection closed.");
        }
        else if (initialize_type_ == 2)
        {
            lsdk_->closeUDP();
            RCLCPP_INFO(this->get_logger(), "UDP connection closed.");
        }
        // 根据SDK文档，如果需要手动释放资源，则取消注释以下行
        // delete lsdk_;
        // lsdk_ = nullptr;
    }
    RCLCPP_INFO(this->get_logger(), "Cleanup in destructor finished.");
}

void UnitreeLidarSDKNode::timer_callback()
{
    int result = lsdk_->runParse();
    static pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

    if (result == LIDAR_IMU_DATA_PACKET_TYPE)
    {
        LidarImuData imu;
        lsdk_->getImuData(imu);

        if (lsdk_->getImuData(imu))
        {
            // IMU消息发布
            rclcpp::Time timestamp(imu.info.stamp.sec, imu.info.stamp.nsec);

            sensor_msgs::msg::Imu imuMsg;
            imuMsg.header.frame_id = imu_frame_;
            imuMsg.header.stamp = timestamp;

            imuMsg.orientation.x = imu.quaternion[0];
            imuMsg.orientation.y = imu.quaternion[1];
            imuMsg.orientation.z = imu.quaternion[2];
            imuMsg.orientation.w = imu.quaternion[3];

            imuMsg.angular_velocity.x = imu.angular_velocity[0];
            imuMsg.angular_velocity.y = imu.angular_velocity[1];
            imuMsg.angular_velocity.z = imu.angular_velocity[2];

            imuMsg.linear_acceleration.x = imu.linear_acceleration[0];
            imuMsg.linear_acceleration.y = imu.linear_acceleration[1];
            imuMsg.linear_acceleration.z = imu.linear_acceleration[2];

            pub_imu_->publish(imuMsg);

            // 发布从 "unilidar_imu_initial" 到 "unilidar_imu" 的TF
            geometry_msgs::msg::TransformStamped transformStamped;
            transformStamped.header.stamp = this->now(); // 使用当前时间
            transformStamped.header.frame_id = imu_frame_ + "_initial"; // 父坐标系
            transformStamped.child_frame_id = imu_frame_; // 子坐标系
            transformStamped.transform.translation.x = 0;
            transformStamped.transform.translation.y = 0;
            transformStamped.transform.translation.z = 0;
            transformStamped.transform.rotation.x = imu.quaternion[1];
            transformStamped.transform.rotation.y = imu.quaternion[2];
            transformStamped.transform.rotation.z = imu.quaternion[3];
            transformStamped.transform.rotation.w = imu.quaternion[0];
            broadcaster_->sendTransform(transformStamped);

            // 发布从 "unilidar_imu" 到 "unilidar_lidar" 的TF
            transformStamped.header.frame_id = imu_frame_; // 父坐标系
            transformStamped.child_frame_id = cloud_frame_; // 子坐标系
            transformStamped.transform.translation.x = 0.007698;
            transformStamped.transform.translation.y = 0.014655;
            transformStamped.transform.translation.z = -0.00667;
            transformStamped.transform.rotation.x = 0;
            transformStamped.transform.rotation.y = 0;
            transformStamped.transform.rotation.z = 0;
            transformStamped.transform.rotation.w = 1;
            broadcaster_->sendTransform(transformStamped);
        }
    }
    else if (result == LIDAR_POINT_DATA_PACKET_TYPE)
    {
        PointCloudUnitree cloud;
        if (lsdk_->getPointCloud(cloud))
        {
            transformUnitreeCloudToPCL(cloud, cloudOut);

            // =================== STEP 1: 在这里添加过滤代码 ===================
            // 这段代码从 processMergedPointCloud 移动而来，并在传感器坐标系下执行
            const double min_range_sq = obs_min_detection_range_xy_ * obs_min_detection_range_xy_;
            const double max_range_sq = obs_max_detection_range_ * obs_max_detection_range_;
            const double fov_rad_half = M_PI * obs_horizontal_fov_deg_ / 360.0;

            pcl::PointCloud<PointType>::Ptr temp_cloud_for_filtering(new pcl::PointCloud<PointType>());
            for (const auto& pt : cloudOut->points) {
                // 在传感器的本地坐标系下，pt.x和pt.y是正确的相对坐标
                double dist_sq_xy = pt.x * pt.x + pt.y * pt.y;
                if (dist_sq_xy >= min_range_sq && dist_sq_xy <= max_range_sq) {
                    double angle_rad = std::atan2(pt.y, pt.x);
                    if (angle_rad >= -fov_rad_half && angle_rad <= fov_rad_half) {
                        temp_cloud_for_filtering->points.push_back(pt);
                    }
                }
            }
            temp_cloud_for_filtering->width = temp_cloud_for_filtering->points.size();
            temp_cloud_for_filtering->height = 1;
            temp_cloud_for_filtering->is_dense = true;
            // 用过滤后的点云覆盖原始点云
            cloudOut = temp_cloud_for_filtering;
            // ========================= 过滤代码结束 =========================

            rclcpp::Time timestamp(
                static_cast<int32_t>(cloud.stamp),
                static_cast<uint32_t>((cloud.stamp - static_cast<int32_t>(cloud.stamp)) * 1e9));

            // 发布原始点云
            sensor_msgs::msg::PointCloud2 cloud_msg;
            pcl::toROSMsg(*cloudOut, cloud_msg);
            cloud_msg.header.frame_id = cloud_frame_;
            cloud_msg.header.stamp = timestamp;
            pub_cloud_->publish(cloud_msg);

            // ********************** 新增TF变换对齐点云 ********************* //
        
            try {
                geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform(
                    "odom", cloud_frame_, timestamp, rclcpp::Duration::from_seconds(1.0)); // 增加等待时间到0.5秒
                RCLCPP_INFO(this->get_logger(), "TF Transform: tx=%f, ty=%f, tz=%f, rx=%f, ry=%f, rz=%f, rw=%f",
                            transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z,
                            transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w);
                Eigen::Affine3f eigen_transform = tf2::transformToEigen(transform.transform).cast<float>();
                pcl::transformPointCloud(*cloudOut, *cloudOut, eigen_transform);
            } catch (tf2::TransformException &ex) {
                RCLCPP_WARN(this->get_logger(), "TF变换失败: %s, 跳过此帧", ex.what());
            }
            // *************************end****************************** //


            // ********************** 新增点云多帧积累 ********************* //
            // 将当前帧加入缓冲区（仅一次）
            cloud_buffer_.push_back(cloudOut->makeShared());

            // 2. 检查缓冲区是否达到kBufferSize（默认5帧）
            if (cloud_buffer_.size() == kBufferSize)
            {
                // 3. 合并缓冲区中的所有帧
                pcl::PointCloud<PointType>::Ptr merged(new pcl::PointCloud<PointType>());
                for (const auto& frag : cloud_buffer_) {
                    *merged += *frag;
                }

                // 4. 处理合并后的点云（过滤、聚类等）
                processMergedPointCloud(merged, timestamp);

                // 5. 清空缓冲区，准备下一次积累
                cloud_buffer_.clear();
            // *************************end****************************** //
            }
        }
    }
}


void UnitreeLidarSDKNode::processMergedPointCloud(pcl::PointCloud<PointType>::Ptr merged, rclcpp::Time timestamp)
{
    RCLCPP_INFO(this->get_logger(), "Merged cloud size: %zu", merged->size());
    // 声明存储障碍物信息
    std::vector<UnitreeLidarSDKNode::ObstacleInfo> obstacle_infos;// 在此处声明，确保作用域正确


    // 定义并应用过滤，生成filtered_cloud
    pcl::PointCloud<PointType>::Ptr filtered_cloud(new pcl::PointCloud<PointType>());

    // 1. 地面去除
    pcl::SACSegmentation<PointType> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(ground_segmentation_distance_threshold_);
    seg.setInputCloud(merged);
    seg.segment(*inliers, *coefficients);
    pcl::ExtractIndices<PointType> extract;
    extract.setInputCloud(merged);
    extract.setIndices(inliers);
    extract.setNegative(true);
    extract.filter(*filtered_cloud);
    RCLCPP_INFO(this->get_logger(), "After ground removal: %zu", filtered_cloud->size());

    // 1. Z轴过滤
    pcl::PassThrough<PointType> pass_z;
    // pass_z.setInputCloud(cloudOut); // 单帧点云
    // pass_z.setInputCloud(merged); // 多帧点云 
    pass_z.setInputCloud(filtered_cloud);
    pass_z.setFilterFieldName("z");
    pass_z.setFilterLimits(static_cast<float>(filtered_cloud_z_min_), static_cast<float>(filtered_cloud_z_max_));
    pass_z.filter(*filtered_cloud);
    RCLCPP_INFO(this->get_logger(), "After Z filter: %zu", filtered_cloud->size());

    // 添加统计滤波
    pcl::StatisticalOutlierRemoval<PointType> sor;
    sor.setInputCloud(filtered_cloud);
    sor.setMeanK(statistical_outlier_removal_mean_k_); // 邻域点数，减小以更严格过滤
    sor.setStddevMulThresh(statistical_outlier_removal_stddev_thresh_); // 标准差阈值，降低以过滤更多离群点
    sor.filter(*filtered_cloud);
    RCLCPP_INFO(this->get_logger(), "After statistical filter: %zu", filtered_cloud->size());

    // // 2. 距离和FOV过滤
    // const double min_range_sq = obs_min_detection_range_xy_ * obs_min_detection_range_xy_;
    // const double max_range_sq = obs_max_detection_range_ * obs_max_detection_range_;
    // const double fov_rad_half = M_PI * obs_horizontal_fov_deg_ / 360.0;

    // pcl::PointCloud<PointType>::Ptr temp_cloud(new pcl::PointCloud<PointType>());
    // for (const auto& pt : filtered_cloud->points) {
    //     double dist_sq_xy = pt.x * pt.x + pt.y * pt.y;
    //     if (dist_sq_xy >= min_range_sq && dist_sq_xy <= max_range_sq) {
    //         double angle_rad = std::atan2(pt.y, pt.x);
    //         if (angle_rad >= -fov_rad_half && angle_rad <= fov_rad_half) {
    //             temp_cloud->points.push_back(pt);
    //         }
    //     }
    // }
    // filtered_cloud = temp_cloud;
    // filtered_cloud->width = filtered_cloud->points.size();
    // filtered_cloud->height = 1;
    // filtered_cloud->is_dense = true;

    // 发布过滤后的点云
    if (!filtered_cloud->empty()) {
        sensor_msgs::msg::PointCloud2 filtered_cloud_msg;
        pcl::toROSMsg(*filtered_cloud, filtered_cloud_msg);
        filtered_cloud_msg.header.frame_id = "odom"; // 使用固定坐标系
        // filtered_cloud_msg.header.frame_id = cloud_frame_;
        filtered_cloud_msg.header.stamp = timestamp;
        pub_filtered_cloud_->publish(filtered_cloud_msg);
        RCLCPP_INFO(this->get_logger(), "Published filtered cloud with %zu points", filtered_cloud->size());

        // 障碍物聚类
        pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
        tree->setInputCloud(filtered_cloud);

        std::vector<pcl::PointIndices> cluster_indices;
        pcl::EuclideanClusterExtraction<PointType> ec;
        ec.setClusterTolerance(euclidean_cluster_tolerance_); // 聚类容差：点之间的最大距离（米）原0.2，减小为0.05以精细化
        ec.setMinClusterSize(euclidean_cluster_min_size_);    // 最小点数：避免噪声，原10保持
        ec.setMaxClusterSize(euclidean_cluster_max_size_);  // 最大点数：限制过大簇，原20000减小为10000
        ec.setSearchMethod(tree);
        ec.setInputCloud(filtered_cloud);
        ec.extract(cluster_indices);
        RCLCPP_INFO(this->get_logger(), "Extracted %zu clusters", cluster_indices.size());

        // 准备发布障碍物位置和可视化
        geometry_msgs::msg::PoseArray obstacle_poses;
        obstacle_poses.header.frame_id = "odom";
        obstacle_poses.header.stamp = timestamp;

        int marker_id = 0; // 用于区分不同的Marker
        for (const auto& cluster : cluster_indices) {
            // 提取每个簇的点云
            pcl::PointCloud<PointType>::Ptr cluster_cloud(new pcl::PointCloud<PointType>);
            for (const auto& idx : cluster.indices) {
                cluster_cloud->push_back(filtered_cloud->points[idx]);
            }
            
            // 计算质心（中心位置）
            Eigen::Vector4f centroid;
            pcl::compute3DCentroid(*cluster_cloud, centroid);
            // RCLCPP_INFO(this->get_logger(), "Centroid: x=%f, y=%f, z=%f",
            //             centroid[0], centroid[1], centroid[2]);
            
            // 计算 AABB 替换 PCA 估计障碍物尺寸
            PointType min_pt, max_pt;
            pcl::getMinMax3D(*cluster_cloud, min_pt, max_pt);
            float length = max_pt.x - min_pt.x; // X 轴方向长度
            float width = max_pt.y - min_pt.y;  // Y 轴方向宽度
            float height = max_pt.z - min_pt.z; // Z 方向高度

            // RCLCPP_INFO(this->get_logger(), "Width calculation: min_pt.y=%f, max_pt.y=%f, width=%f",
            //             min_pt.y, max_pt.y, width);
                        
            // 获取传感器在odom坐标系中的位置
            geometry_msgs::msg::TransformStamped sensor_transform;
            try {
                sensor_transform = tf_buffer_->lookupTransform(
                    "odom", cloud_frame_, timestamp, rclcpp::Duration::from_seconds(1.0));
               RCLCPP_INFO(this->get_logger(), "激光雷达位置: x=%f, y=%f, z=%f",
                        sensor_transform.transform.translation.x,
                        sensor_transform.transform.translation.y,
                        sensor_transform.transform.translation.z);
            } catch (tf2::TransformException &ex) {
                RCLCPP_WARN(this->get_logger(), "Failed to get sensor transform: %s, using (0,0,0)", ex.what());
                sensor_transform.transform.translation.x = 0.0;
                sensor_transform.transform.translation.y = 0.0;
                sensor_transform.transform.translation.z = 0.0;
            }
            Eigen::Vector3f sensor_pos(
                sensor_transform.transform.translation.x,
                sensor_transform.transform.translation.y,
                sensor_transform.transform.translation.z
            );

            // 计算距离（从传感器到质心的欧几里得距离）
            float distance = std::sqrt(
                (centroid[0] - sensor_pos[0]) * (centroid[0] - sensor_pos[0]) +
                (centroid[1] - sensor_pos[1]) * (centroid[1] - sensor_pos[1]) +
                (centroid[2] - sensor_pos[2]) * (centroid[2] - sensor_pos[2])
            );
            // float corrected_distance = distance - 0.1; // 减去10cm偏差
            // if (corrected_distance < 0.0) corrected_distance = 0.0; 
            float corrected_distance = distance; // 无校正
            
            // 计算障碍物在 Y 轴上的左右分布距离（基于 AABB 边界）
            float left_distance = std::abs(min_pt.y - sensor_pos[1]);  // 左侧距离
            float right_distance = std::abs(max_pt.y - sensor_pos[1]); // 右侧距离

            // 确保距离非负
            if (left_distance < 0) left_distance = 0.0f;
            if (right_distance < 0) right_distance = 0.0f;

            // 调试：打印雷达位置和障碍物边界
            // RCLCPP_INFO(this->get_logger(), "Sensor Pos: (%f, %f, %f), Min Pt: (%f, %f, %f), Max Pt: (%f, %f, %f)",
            //         sensor_pos[0], sensor_pos[1], sensor_pos[2],
            //         min_pt.x, min_pt.y, min_pt.z,
            //         max_pt.x, max_pt.y, max_pt.z);

            // 将障碍物信息存储到向量中，包括左右距离和质心
            obstacle_infos.push_back({corrected_distance, width, height, length, left_distance, right_distance, centroid});
            
            // 发布障碍物位置（质心）
            geometry_msgs::msg::Pose pose;
            pose.position.x = centroid[0];
            pose.position.y = centroid[1];
            pose.position.z = centroid[2];
            pose.orientation.w = 1.0; // 默认朝向
            obstacle_poses.poses.push_back(pose);
            
            // 可视化边界框
            visualization_msgs::msg::Marker marker;
            marker.header.frame_id = "odom";
            marker.header.stamp = timestamp;
            marker.ns = "obstacles";
            marker.id = marker_id++;
            marker.type = visualization_msgs::msg::Marker::CUBE; // 使用立方体表示边界框
            marker.action = visualization_msgs::msg::Marker::ADD;
            marker.pose.position.x = centroid[0];
            marker.pose.position.y = centroid[1];
            marker.pose.position.z = centroid[2];
            marker.scale.x = length;
            marker.scale.y = width;
            marker.scale.z = height;
            marker.color.r = 0.0; // 绿色
            marker.color.g = 1.0;
            marker.color.b = 0.0;
            marker.color.a = 0.5; // 半透明
            marker.lifetime = rclcpp::Duration::from_seconds(0.1); // 持续时间，避免重叠
            pub_marker_->publish(marker);

            // 新增：可视化障碍物参数（距离、宽度、高度、厚度）
            visualization_msgs::msg::Marker text_marker;
            text_marker.header.frame_id = "odom";
            text_marker.header.stamp = timestamp;
            text_marker.ns = "obstacle_params";
            text_marker.id = marker_id + 1000; // 避免与边界框ID冲突
            text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
            text_marker.action = visualization_msgs::msg::Marker::ADD;
            text_marker.pose.position.x = centroid[0];
            text_marker.pose.position.y = centroid[1];
            text_marker.pose.position.z = centroid[2] + height + 0.2; // 放置在边界框上方
            text_marker.scale.z = 0.2; // 文本高度（单位：米）
            text_marker.color.r = 1.0; // 白色
            text_marker.color.g = 1.0;
            text_marker.color.b = 1.0;
            text_marker.color.a = 1.0;
            std::stringstream ss;
            ss << "Dist(距离): " << std::fixed << std::setprecision(2) << corrected_distance << " m\n"
               << "Width(宽度): " << width << " m\n"
               << "Height(高度): " << height << " m\n"
               << "Length(长度): " << length << " m\n"
               << "Left(左侧): " << left_distance << " m\n"
               << "Right(右侧): " << right_distance << " m";
            text_marker.text = ss.str();
            text_marker.lifetime = rclcpp::Duration::from_seconds(0.1);

            // 在终端打印障碍物参数
            // RCLCPP_INFO(this->get_logger(), "Obstacle %d:\n%s", marker_id, ss.str().c_str());
            pub_marker_->publish(text_marker);

            marker_id++; // 递增Marker ID
        }
        
        // 发布所有障碍物的位置
        pub_obstacles_->publish(obstacle_poses);
        RCLCPP_INFO(this->get_logger(), "检测到 %zu 个障碍物", cluster_indices.size());
        
        // 找到并打印最近障碍物信息，包括位置判断和避障建议
        if (!obstacle_infos.empty()) {
            auto nearest = std::min_element(obstacle_infos.begin(), obstacle_infos.end(),
                [](const ObstacleInfo& a, const ObstacleInfo& b) {
                    return a.distance < b.distance;
                });

            // 获取设备的姿态（从odom到unilidar_imu的TF变换）
            geometry_msgs::msg::TransformStamped base_transform;
            try {
                base_transform = tf_buffer_->lookupTransform("odom", "unilidar_imu", timestamp, rclcpp::Duration::from_seconds(1.0));
            } catch (tf2::TransformException &ex) {
                RCLCPP_WARN(this->get_logger(), "Failed to get unilidar_imu transform: %s", ex.what());
                return;
            }

            // 转换最近障碍物的质心到unilidar_imu坐标系
            geometry_msgs::msg::PointStamped centroid_odom;
            centroid_odom.header.frame_id = "odom";
            centroid_odom.point.x = nearest->centroid[0];
            centroid_odom.point.y = nearest->centroid[1];
            centroid_odom.point.z = nearest->centroid[2];

            geometry_msgs::msg::PointStamped centroid_base;
            tf2::doTransform(centroid_odom, centroid_base, base_transform);

            // 判断障碍物在设备的左侧还是右侧
            std::string side = (centroid_base.point.y > 0) ? "左侧" : "右侧";

            // 计算避障移动距离
            float robot_width = 0.5; // 设备宽度，需根据实际情况设置
            float safety_distance = 0.5; // 安全距离，需根据需求调整
            float move_distance;
            std::string direction;
            if (centroid_base.point.y < 0) {
                // 障碍物在左侧，向右移动
                move_distance = std::abs(centroid_base.point.y + nearest->width / 2) + robot_width / 2 + safety_distance;
                direction = "向右";
            } else {
                // 障碍物在右侧，向左移动
                move_distance = (centroid_base.point.y - nearest->width / 2) + robot_width / 2 + safety_distance;
                direction = "向左";
            }

            // 打印最近障碍物信息，包括位置和避障建议
            RCLCPP_INFO(this->get_logger(), "最近障碍物:\n"
                        "Dist(距离): %.2f m\n"
                        "Width(宽度): %.2f m\n"
                        "Height(高度): %.2f m\n"
                        "Length(长度): %.2f m\n"
                        "右侧距离: %.2f m\n"
                        "左侧距离: %.2f m\n"
                        "位置: 设备%s\n"
                        "避障建议: %s移动 %.2f 米",
                        nearest->distance, nearest->width, nearest->height, nearest->length,
                        nearest->left_distance, nearest->right_distance,
                        side.c_str(), direction.c_str(), move_distance);
        } else {
            RCLCPP_INFO(this->get_logger(), "没有检测到障碍物");
        }
    } else {
        RCLCPP_WARN(this->get_logger(), "过滤后的点云为空，未发布");
    }
}


// 主函数
int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<UnitreeLidarSDKNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}